% Rappaport Lab
% August 23 2017
% This script will take a color and depth image from a Kinect and plot the
% point cloud in Matlab to enstablish a ground truth for comparison to the
% AIT imaging

%% BEGIN SCRIPT

% start clocks;
tic_1 = tic;
tic_2 = tic;
tic_3 = tic;

% create a System object for the color component of the Kinect
colorDevice = imaq.VideoDevice('kinect',1);

% create a System object for the depth component of the Kinect
depthDevice = imaq.VideoDevice('kinect',2);

% initialize the Kinect's cameras
step(colorDevice);
step(depthDevice);

% take a simple frame of an image
colorImage = step(colorDevice);
depthImage = step(depthDevice);

% extract the point cloud
ptCloud = pcfromkinect(depthDevice,depthImage,colorImage);

% time to get point cloud
toc(tic_1);

% initialize a point cloud player to visualize 3-D point cloud data
player = pcplayer(ptCloud.XLimits,ptCloud.YLimits,ptCloud.ZLimits,...
	'VerticalAxis','y','VerticalAxisDir','down');

% label axes
xlabel(player.Axes,'X (m)');
ylabel(player.Axes,'Y (m)');
zlabel(player.Axes,'Z (m)');

% plot the point cloud
view(player,ptCloud);

% total time to plot the point cloud
toc(tic_2);

%% BELOW IS FOR SLECTIVELY SEEING PORTIONS OF THE POINT CLOUD THAT HAVE THE
% OBJECTS THAT WE ARE IMAGING

% syntax for the Region of Interest (roi)
% roi = [xmin, xmax; ymin, ymax; zmin, zmax];
roi = [-1, 1; 0, 1; 0, 4];

% linear indices of the point cloud within the region of interest
indices = findPointsInROI(ptCloud,roi);

% get the point cloud with only those indices
ptCloudB = select(ptCloud,indices);

% plot the point cloud
figure(4)
pcshow(ptCloudB.Location);

%% USING PLANES TO SELECT THE AREA TO VIEW

% % max distance that the point cloud will look
% maxDistance = 1;
% 
% % vector determining which direction to look
% referenceVector = [1,0,0];
% 
% % angular distance to look
% maxAngularDistance = 5;
% 
% % select the portion of the point cloud to look at
% [model1,inlierIndices,outlierIndices] = pcfitplane(ptCloud,...
%             maxDistance,referenceVector,maxAngularDistance);
% plane1 = select(ptCloud,inlierIndices);
% 
% % select the remaining portions of the point cloud
% remainPtCloud = select(ptCloud,outlierIndices);
% 
% % show selected portion of the point cloud
% figure(2)
% pcshow(plane1)
% title('The Imaged Object')
% axis([-1 1 -1 1 1.5 2.5]);

% % show remaining portion of the point cloud
% figure(3)
% pcshow(remainPtCloud)
% title('Remaining Planes')

%% UNCOMMENT TO TAKE REAL-TIME IMAGING USING THE KINECT

% to acquire 500 frames of data...
% for i = 1:500    
%    colorImage = step(colorDevice);  
%    depthImage = step(depthDevice);
%  
%    ptCloud = pcfromkinect(depthDevice,depthImage,colorImage);
%  
%    view(player,ptCloud);
% end

%% CLEAR VARIABLES AND END SCRIPT

% clear variables
release(colorDevice);
release(depthDevice);

% total time of script
toc(tic_3);

% END SCRIPT